#include "drv_duo.h"
#include "drv_systick.h"
#include "stdio.h"
#include "drv_systick.h"

// FRONT_LEFT 1
// BACK_LEFT 2
// FRONT_RIGHT 3
// BACK_RIGHT 4

//PWM参数

#define MIN_PULSE_WIDTH 1000 //0.5ms
#define MAX_PULSE_WIDTH 5000 //2.5ms                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
#define CENTER_PLUSE_WIDTH 3000 //1.5ms

void TIM1_INIT(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
	TIM_BDTRInitTypeDef  TIM_BDTRInitStruct;

	//开启时钟：GPIOA、TIM1
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);

	//配置GPIOA8~A11为复用推挽输出，接TIM1_CH1~CH4
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;          // 复用功能
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;        // 推挽输出
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9| GPIO_Pin_10| GPIO_Pin_11; // TIM1的4个通道
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;      // 无上下拉
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;    // 高速

	GPIO_Init(GPIOA,&GPIO_InitStruct);

	//将GPIO与TIM1通道复用映射
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource8,GPIO_AF_TIM1);  // TIM1_CH1
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource9,GPIO_AF_TIM1);  // TIM1_CH2
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource10,GPIO_AF_TIM1); // TIM1_CH3
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource11,GPIO_AF_TIM1); // TIM1_CH4
	
	//配置TIM1基本定时器
	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频=1
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数
	TIM_TimeBaseInitStruct.TIM_Period = 40000-1;  // 自动重装值ARR -> 控制PWM频率
	TIM_TimeBaseInitStruct.TIM_Prescaler = 84-1;// 预分频PSC -> 降低计数频率
	TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0; // 重复计数器（高级定时器功能）
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);

	//配置输出比较（PWM模式）
	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;        // PWM模式1
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;// 高电平有效
	TIM_OCInitStruct.TIM_Pulse = 0;                       // 初始占空比=0
	TIM_OCInitStruct.TIM_OutputState = ENABLE;            // 使能输出
	TIM_OCInitStruct.TIM_OCNIdleState = TIM_OCNIdleState_Reset; // 空闲状态：复位
	TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Reset;   // 空闲状态：复位
	TIM_OCInitStruct.TIM_OutputNState = TIM_OutputNState_Disable; // 关闭互补输出
	TIM_OCInitStruct.TIM_OCNPolarity = TIM_OCNPolarity_High;     // 占位（互补输出极性）

	//初始化4个通道（CH1~CH4）
	TIM_OC1Init(TIM1, &TIM_OCInitStruct);
	TIM_OC2Init(TIM1, &TIM_OCInitStruct);
	TIM_OC3Init(TIM1, &TIM_OCInitStruct);
	TIM_OC4Init(TIM1, &TIM_OCInitStruct);
	
	//开启影子寄存器（使修改生效需更新事件）
	TIM_ARRPreloadConfig(TIM1, ENABLE);          // 自动重装寄存器影子
	TIM_OC1PreloadConfig(TIM1,  TIM_OCPreload_Enable); // CH1影子比较寄存器
	TIM_OC2PreloadConfig(TIM1,  TIM_OCPreload_Enable); // CH2影子比较寄存器
	TIM_OC3PreloadConfig(TIM1,  TIM_OCPreload_Enable); // CH3影子比较寄存器
	TIM_OC4PreloadConfig(TIM1,  TIM_OCPreload_Enable); // CH4影子比较寄存器	

	//高级定时器BDTR配置（死区、刹车、输出控制）
	TIM_BDTRInitStruct.TIM_AutomaticOutput = TIM_AutomaticOutput_Enable; // 自动输出使能
	TIM_BDTRInitStruct.TIM_Break = TIM_Break_Disable;                    // 禁用刹车输入
	TIM_BDTRInitStruct.TIM_BreakPolarity = TIM_BreakPolarity_High;       // 刹车极性（无效）
	TIM_BDTRInitStruct.TIM_DeadTime = 0;                                 // 无死区时间
	TIM_BDTRInitStruct.TIM_LOCKLevel = TIM_LOCKLevel_OFF;                 // 不锁定
	TIM_BDTRInitStruct.TIM_OSSIState = TIM_OSSIState_Enable;              // 空闲时输出使能
	TIM_BDTRInitStruct.TIM_OSSRState = TIM_OSSRState_Enable;              // 运行模式下输出使能
	
	TIM_BDTRConfig(TIM1, & TIM_BDTRInitStruct);

	//主输出使能（MOE位），必须打开才能真正输出PWM
	TIM_CtrlPWMOutputs(TIM1,ENABLE);
	//启动定时器
	TIM_Cmd(TIM1, ENABLE);
}

uint16_t pulsewidth=0;

//舵机角度
void setservo_angle(uint8_t servo,uint16_t angle)
{
	if(servo==4||servo==3)
	{
		angle=180-angle;
	}																																																																																																																																								
	pulsewidth = mapangletopulse(angle);
	if(pulsewidth < MIN_PULSE_WIDTH)	pulsewidth = MIN_PULSE_WIDTH;
	if(pulsewidth > MAX_PULSE_WIDTH)	pulsewidth = MAX_PULSE_WIDTH;
	switch(servo)
    {
        case 1: TIM_SetCompare1(TIM1, pulsewidth); break;  // 仅控制1号舵机
        case 2: TIM_SetCompare2(TIM1, pulsewidth); break;  // 仅控制2号舵机
        case 3: TIM_SetCompare3(TIM1, pulsewidth); break;  // 仅控制3号舵机
        case 4: TIM_SetCompare4(TIM1, pulsewidth); break;  // 仅控制4号舵机
        default: break;  // 无效舵机号不操作
    }
}
//设置占空比
uint16_t mapangletopulse(uint16_t angle)
{
	return MIN_PULSE_WIDTH + (angle *(MAX_PULSE_WIDTH - MIN_PULSE_WIDTH)/180 );
}

// 状态机变量
DogState current_state = STATE_IDLE;
uint8_t action_step = 0;
uint16_t action_counter = 0;
uint8_t action_repeat_count = 0;

//站立
void action_standup(void)
{
			setservo_angle(1,90);
			setservo_angle(2,90);
		
			setservo_angle(3,90);
			setservo_angle(4,90);
}
//趴下
void action_liedown(void)
{
			setservo_angle(1,0);
	
			setservo_angle(2,0);
		
			setservo_angle(3,0);
	
			setservo_angle(4,0);
}

// 更新小狗动作的状态机函数（在主循环中定期调用）
void update_dog_action(void)
{
    switch (current_state)
    {
        case STATE_IDLE:
            // 空闲状态，什么都不做
            break;
            
        case STATE_STANDUP:
            action_standup();
            current_state = STATE_IDLE;
            break;
            
        case STATE_LIEDOWN:
            action_liedown();
            current_state = STATE_IDLE;
            break;
            
        case STATE_FRONT:
            switch (action_step)
            {
                case 0:
                    // 第一步：设置舵机角度
                    setservo_angle(1, 135);
                    setservo_angle(4, 134);
                    setservo_angle(2, 45);
                    setservo_angle(3, 45);
                    action_counter = 0;
                    action_step = 1;
                    break;
                    
                case 1:
                    // 等待一段时间（通过计数器实现）
                    action_counter++;
                    if (action_counter >= 50) // 大约500ms，根据主循环速度调整
                    {
                        // 第二步：改变舵机角度
                        setservo_angle(1, 45);
                        setservo_angle(4, 45);
                        setservo_angle(2, 135);
                        setservo_angle(3, 135);
                        action_counter = 0;
                        action_step = 2;
                    }
                    break;
                    
                case 2:
                    action_counter++;
                    if (action_counter >= 50) // 大约500ms
                    {
                        // 重复次数检查
                        action_repeat_count++;
                        if (action_repeat_count >= 10)
                        {
                            // 完成10次循环，回到中心位置并重置状态
                            setservo_angle(1, 90);
                            setservo_angle(2, 90);
                            setservo_angle(3, 90);
                            setservo_angle(4, 90);
                            current_state = STATE_IDLE;
                            action_step = 0;
                            action_repeat_count = 0;
                        }
                        else
                        {
                            // 继续下一次循环
                            action_step = 0;
                        }
                    }
                    break;
            }
            break;
            
        case STATE_BACK:
            // 类似STATE_FRONT，实现后退的状态机
            // 这里需要根据原有的action_back函数实现
            // 由于代码较长，省略具体实现，但结构与STATE_FRONT类似
            switch (action_step)
            {
                case 0:
                    setservo_angle(1, 45);
                    setservo_angle(4, 45);
                    setservo_angle(2, 135);
                    setservo_angle(3, 135);
                    action_counter = 0;
                    action_step = 1;
                    break;
                    
                case 1:
                    action_counter++;
                    if (action_counter >= 50)
                    {
                        setservo_angle(1, 90);
                        setservo_angle(4, 90);
                        setservo_angle(2, 90);
                        setservo_angle(3, 90);
                        action_counter = 0;
                        action_step = 2;
                    }
                    break;
                    
                case 2:
                    action_counter++;
                    if (action_counter >= 50)
                    {
                        setservo_angle(2, 135);
                        setservo_angle(3, 135);
                        setservo_angle(2, 45);
                        setservo_angle(3, 45);
                        action_counter = 0;
                        action_step = 3;
                    }
                    break;
                    
                case 3:
                    action_counter++;
                    if (action_counter >= 50)
                    {
                        action_repeat_count++;
                        if (action_repeat_count >= 10)
                        {
                            setservo_angle(1, 90);
                            setservo_angle(2, 90);
                            setservo_angle(3, 90);
                            setservo_angle(4, 90);
                            current_state = STATE_IDLE;
                            action_step = 0;
                            action_repeat_count = 0;
                        }
                        else
                        {
                            action_step = 0;
                        }
                    }
                    break;
            }
            break;
    }
}

// 重置动作状态（用于中断当前动作）
void reset_dog_action(void)
{
    action_step = 0;
    action_counter = 0;
    action_repeat_count = 0;
}


